#include <winsock2.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <stdint.h>
#include "TCPServer.h"

#include "nxt.h"

using namespace std;

//Client thread, each client gets his own thread.
//With the thread the cpu checks each cyle a certain procent for new messages
DWORD WINAPI ClientThread(LPVOID lpParam)
{
	SOCKET        sock=(SOCKET)lpParam;
	bool HaveCamCentre = false;
	int CamCentreY = 0;
	int CamCentreX = 0;
	int           ret;
	int NumX = 0;
	int NumY = 0;
	int Height = 0;
	int Width = 0;
	double PixelAngleRatio = 0;
	int CurrentPositionY;
	int CurrentPositionX;
	double MotorX;
	int MotorXInt;
	ComStruct *Message;
	Message = (ComStruct *) malloc(sizeof(ComStruct));
	(*Message).x = 0;
	(*Message).y = 0;

	Connection *connection = new Bluetooth();
	Brick *nxt = new Brick(connection);
	Motor *motorA = new Motor(OUT_A, connection);
    connection->connect(3);

	//First set is the cam centre coordenates
	if(!HaveCamCentre){
		cout << "Checking centre..." << endl;
		//Recieve data
		ret = recv(sock, (char*)Message, sizeof(ComStruct), 0);

		 //Converd the recieved data from network byte order to host byte order.
		 CamCentreY = ntohs(Message->y);
		 CamCentreX = ntohs(Message->x);
		 Width = ntohs(Message->Witdh);
         Height = ntohs(Message->Height);
         cout << "Width: " << Width << endl;
         PixelAngleRatio = 180.00 / (double)Width;
         cout << "Pixel angle ratio is: " << PixelAngleRatio << endl;
		 Message->x = 0;
		 Message->y = 0;
         cout << "Centre is y is: " << CamCentreY << endl;
	     cout << "Centre is x is: " << CamCentreX << endl;
		 HaveCamCentre = true;
	};

         CurrentPositionX = CamCentreX;
         CurrentPositionY = CamCentreY;
         //motorA->move_to(10, 90, 5);
        // motorA->move_to(10, -90, 5);
         motorA->stop();
         motorA->reset_rotation();
         cout <<"========= Rotation: " << motorA->get_rotation() << " =========" << endl;
         cout << "Turned" << endl;

	while(HaveCamCentre && ret != -1){

		 //Recieve data
		 ret = recv(sock, (char*)Message, sizeof(ComStruct), 0);
		 NumY = ntohs(Message->y);
		 NumX= ntohs(Message->x);
		 cout << "Y is: " << NumY << endl;
	     cout << "X is: " << NumX << endl;

         if((NumY !=0) || (NumX !=0)){


		 // Check if X motor needs to be accuated.
		 if(NumX > CurrentPositionX ){
			 cout << "Move X motor RIGHT" << endl;
			 //Get distance between the current and the new position
			 CurrentPositionX = NumX - CurrentPositionX;
			 MotorX = (double)CurrentPositionX * PixelAngleRatio;
             cout << "Motor X is: " << MotorX << endl;
             MotorXInt = (int) MotorX;
             motorA->on(20);
             Sleep(200);
             motorA->off();
             cout << "Motor X  int is: " << MotorXInt << endl;
			 //motorA->move_to(5, MotorXInt, 5);
			 cout << "DONE RIGHT" << endl;
		 }else if(NumX < CurrentPositionX){
			cout << "Move X motor LEFT" << endl;
			 CurrentPositionX = NumX - CurrentPositionX;
			 MotorX = (double)CurrentPositionX * PixelAngleRatio;
			 MotorX = 0.00 - MotorX;
			 cout << "Motor X is: " << MotorX << endl;
             MotorXInt = (int) MotorX;
             motorA->on(-20);
             Sleep(200);
             motorA->off();
             cout << "Motor X  int is: " << MotorXInt << endl;
			 //motorA->move_to(5, MotorXInt, 5);
             cout << "DONE LEFT" << endl;
		 };

         }
         else{
           cout << "NumY or NumX is zero, do nothing" << endl;
         };

         CurrentPositionX = NumX;
         CurrentPositionY = NumY;
	};
	//connection->disconnect();
	return 0;
}

int SetupServer(string Adres, int Port){
	WSADATA       wsd;
	SOCKET        sListen,
		sClient;
	int           iAddrSize;
	HANDLE        hThread;
	DWORD         dwThreadId;
	struct sockaddr_in local,
		client;

	if (WSAStartup(MAKEWORD(2,2), &wsd) != 0){
		printf("Failed to load Winsock!\n");
		return 1;
	};

	// Create  socket to lisen on
	sListen = socket(AF_INET, SOCK_STREAM, IPPROTO_IP);

	if (sListen == SOCKET_ERROR){
		printf("socket() failed: %d\n", WSAGetLastError());
		return 1;
	};

	// Select the local interface and bind to it
	local.sin_addr.s_addr = inet_addr(Adres.c_str());
	local.sin_family = AF_INET;
	local.sin_port = htons(Port);

	if (bind(sListen, (struct sockaddr *)&local,sizeof(local)) == SOCKET_ERROR){
		printf("bind() failed: %d\n", WSAGetLastError());
		return 1;
	};

	listen(sListen, 8);

	// For each new client check make a thread.
	while (1)
	{
		cout << "In server loop" << endl;
		iAddrSize = sizeof(client);
		sClient = accept(sListen, (struct sockaddr *)&client,
			&iAddrSize);
		if (sClient == INVALID_SOCKET)
		{
			printf("accept() failed: %d\n", WSAGetLastError());
			break;
		}
		printf("Accepted client: %s:%d\n",
			inet_ntoa(client.sin_addr), ntohs(client.sin_port));

		hThread = CreateThread(NULL, 0, ClientThread,
			(LPVOID)sClient, 0, &dwThreadId);
		if (hThread == NULL)
		{
			printf("CreateThread() failed: %d\n", GetLastError());
			break;
		}

		CloseHandle(hThread);
	}

	// Close and cleanup
	closesocket(sListen);
	WSACleanup();

	return 0;
};

int main(int argc, char **argv)
{
	// Setup server and run
	SetupServer("192.168.0.2", 5000);

	return 0;
}
